#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>


//#include "include/world_model.h"
#include "world_model.hpp"

int
main (int argc, char** argv)
{


  pcl::WorldModel<pcl::PointXYZI> wm;

  std::cout << "current resolution: " << wm.getResolution() << std::endl;


  std::cout << "setting resolution to 0.1 " << std::endl;
  wm.setResolution(0.1f);

  std::cout << "current resolution: " << wm.getResolution() << std::endl;

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);

  if(argc < 2) {std::cout << "No pcd to read... Exiting..." << std::endl;  return (-1); }

  if (pcl::io::loadPCDFile<pcl::PointXYZI> (argv[1], *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file %s \n", argv[1]);
    return (-1);
  }

  wm.addCloud(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, cloud);

  std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds;
  std::vector<Eigen::Vector3f> transforms;
    
  wm.getWorldAsCubes(256.0, clouds, transforms);

  int cpt_name = 0;

  for(int i = 0 ; i < clouds.size(); ++i)
  {
    std::stringstream name;
    name << "cube_" << cpt_name << ".pcd";
    std::cout << "Saving " << name.str() << std::endl;

    if(   (*(clouds[i])).points.size() <= 0) {std::cout << "Cube is empty" << std::endl; continue; }
    
    pcl::io::savePCDFileASCII (name.str(), *(clouds[i]));
    cpt_name++;
  }


  return (0);
}
